#include "bodyarch.h"

BodyArch::BodyArch()
{
	settings = Settings::Instance();
	totalWeight = 0.0f;
	bodypartspacer = 2.2f;
}

void BodyArch::buildArch()
{
	// Create a head first.
		archBodyparts.push_back( archBodypart() );
		archBodypart *bp = &archBodyparts[archBodyparts.size()-1];
		bp->id		= 1000;
		bp->type	= archBodypart::HEAD;
		bp->materialID	= 0;
		bp->x		= randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );
		bp->y		= randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );
		bp->z		= randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );

  // Code below changed to handle "Bodyless" head-only critter, but this will prevent that from happening anyhow:
  addRandomBodypart();

	// add random more
		unsigned int runs = randgen->Instance()->get( settings->getCVar("body_minbodypartsatbuildtime"), settings->getCVar("body_maxbodypartsatbuildtime")-1 ); // -1 -> central bodypart
		for ( unsigned int i=0; i < runs; i++ )
			addRandomBodypart();

}

void BodyArch::removeBodypart(unsigned int id)
{
	// find constraints where this bodypart is id1, in order to remove connected bodyparts
	for ( unsigned int i = 0; i < archConstraints.size(); i++ )
	{
		archConstraint* c = &archConstraints[i];
		if ( c->id_1 == id )
		{
				removeBodypart( c->id_2 );
		}
	}

	archBodyparts.erase( archBodyparts.begin() + findBodypart(id) );
}

void BodyArch::addRandomBodypart()
{
	// Throw in a bodypart
		archBodyparts.push_back( archBodypart() );
		archBodypart *bp = &archBodyparts[archBodyparts.size()-1];
		bp->id		= 0; // to avoid uninitialized id
		bp->id		= getUniqueBodypartID();
		bp->type	= archBodypart::DEFAULT;
		bp->materialID	= 0;
		bp->x		= randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );
		bp->y		= randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );
		bp->z		= randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );

    unsigned int maxheads = settings->getCVar("body_maxheads");
    unsigned int currentheads = 0;
    for(int i=0;i<archBodyparts.size();i++)
      if(archBodyparts[i].type == archBodypart::HEAD)
        ++currentheads;

    if(maxheads > currentheads)
    {
      int ishead = randgen->Instance()->get(0, 100);
      if(settings->getCVar("body_percentchance_head") > ishead)
        bp->type = archBodypart::HEAD;
    }

	// Get it connected somehow
		unsigned int connID1 = randgen->Instance()->get( 0, archBodyparts.size()-1 );
		unsigned int connID2 = archBodyparts.size()-1;
		while ( connID1 == connID2 )
			connID1 = randgen->Instance()->get( 0, archBodyparts.size()-1 );

		addRandomConstraint(connID1, connID2, false);
}

void BodyArch::addRandomConstraint(unsigned int connID1, unsigned int connID2, bool isMouth)
{
		archConstraints.push_back( archConstraint() );
		archConstraint *co = &archConstraints[archConstraints.size()-1];

		// first initialize constraint id's
		co->constraint_id1	= 0;
		co->constraint_id2	= 0;
		co->constraint_id1	= getUniqueConstraintID();
		co->constraint_id2	= getUniqueConstraintID();
		co->id_1		= archBodyparts[ connID1 ].id;
		co->id_2		= archBodyparts[ connID2 ].id;

		co->rot_x_1		= ((float)randgen->Instance()->get( 0, 1571 ) - 0) / 1000;
		co->rot_y_1		= ((float)randgen->Instance()->get( 0, 1571 ) - 0) / 1000;
		co->rot_z_1		= ((float)randgen->Instance()->get( 0, 1571 ) - 0) / 1000;

		co->rot_x_2		= ((float)randgen->Instance()->get( 0, 1571 ) - 0) / 1000;
		co->rot_y_2		= ((float)randgen->Instance()->get( 0, 1571 ) - 0) / 1000;
		co->rot_z_2		= ((float)randgen->Instance()->get( 0, 1571 ) - 0) / 1000;

		randomConstraintPosition(co, 1, connID1);
		randomConstraintPosition(co, 2, connID2);

		co->limit_1		= (float)randgen->Instance()->get( 0, 7853 ) / -10000;
		co->limit_2		= -1.0f * co->limit_1;
}

void BodyArch::randomConstraintPosition(archConstraint* co, unsigned int OneOrTwo, unsigned int connID)
{
	if ( OneOrTwo == 1 )
	{
		co->XYZ = randgen->Instance()->get( 0, 2 );
		co->sign = randgen->Instance()->get( 0, 1 );
		if ( co->sign == 0 ) co->sign = -1;

	// now we know the plane to connect to, determine positions
		if ( co->XYZ == 0 ) // X
		{
			// ((x / 1000.0f)  / 2)  * 1.5f * co->sign = 
			co->pos_x_1 = (archBodyparts[connID].x / 2000.0f) * co->sign * bodypartspacer;
			co->pos_y_1 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].y)*2) - archBodyparts[connID].y) / 1000;
			co->pos_z_1 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].z)*2) - archBodyparts[connID].z) / 1000;
		}
		else if ( co->XYZ == 1 ) // Y
		{
			co->pos_x_1 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].x)*2) - archBodyparts[connID].x) / 1000;
			co->pos_y_1 = (archBodyparts[connID].y / 2000.0f) * co->sign * bodypartspacer;
			co->pos_z_1 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].z)*2) - archBodyparts[connID].z) / 1000;
		}
		else // Z
		{
			co->pos_x_1 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].x)*2) - archBodyparts[connID].x) / 1000;
			co->pos_y_1 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].y)*2) - archBodyparts[connID].y) / 1000;
			co->pos_z_1 = (archBodyparts[connID].z / 2000.0f) * co->sign * bodypartspacer;
		}
	}
	else
	{
		int othersign = -1 * co->sign;
		if ( co->XYZ == 0 ) // X
		{
			co->pos_x_2 = (archBodyparts[connID].x / 2000.0f) * othersign * bodypartspacer;
			co->pos_y_2 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].y)*2) - archBodyparts[connID].y) / 1000;
			co->pos_z_2 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].z)*2) - archBodyparts[connID].z) / 1000;
		}
		else if ( co->XYZ == 1 ) // Y
		{
			co->pos_x_2 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].x)*2) - archBodyparts[connID].x) / 1000;
			co->pos_y_2 = (archBodyparts[connID].y / 2000.0f) * othersign * bodypartspacer;
			co->pos_z_2 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].z)*2) - archBodyparts[connID].z) / 1000;
		}
		else // Z
		{
			co->pos_x_2 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].x)*2) - archBodyparts[connID].x) / 1000;
			co->pos_y_2 = ((float)randgen->Instance()->get( 0, (archBodyparts[connID].y)*2) - archBodyparts[connID].y) / 1000;
			co->pos_z_2 = (archBodyparts[connID].z / 2000.0f) * othersign * bodypartspacer;
		}
	}
}

void BodyArch::mutate(unsigned int runs)
{
	for ( unsigned int i=0; i < runs; i++ )
	{
		unsigned int tsum = 	settings->getCVar("body_percentmutateeffectchangecolor")
					+ settings->getCVar("body_percentmutateeffectchangecolor_slightly")
					+ settings->getCVar("body_percentmutateeffectaddbodypart")
					+ settings->getCVar("body_percentmutateeffectremovebodypart")
					+ settings->getCVar("body_percentmutateeffectresizebodypart")
					+ settings->getCVar("body_percentmutateeffectresizebodypart_slightly")
					+ settings->getCVar("body_percentmutateeffectchangeconstraintlimits")
					+ settings->getCVar("body_percentmutateeffectchangeconstraintlimits_slightly")
					+ settings->getCVar("body_percentmutateeffectchangeconstraintangles")
					+ settings->getCVar("body_percentmutateeffectchangeconstraintangles_slightly")
					+ settings->getCVar("body_percentmutateeffectchangeconstraintposition")
					+ settings->getCVar("body_percentmutateeffectchangeconstraintposition_slightly")
				;

		unsigned int mode = randgen->Instance()->get(1,tsum);

		// CHANGE COLOR
			unsigned int modesum = settings->getCVar("body_percentmutateeffectchangecolor");
			if ( mode <= modesum )
			{
        //cerr << "Mutate color." << endl;
				// mutate color
				unsigned int ncolor = randgen->Instance()->get(0,2);

				if ( ncolor == 0 )
					color.r = (float)BeRand::Instance()->get(0,100)/100.0f;
				else if ( ncolor == 1 )
					color.g = (float)BeRand::Instance()->get(0,100)/100.0f;
				else if ( ncolor == 2 )
					color.b = (float)BeRand::Instance()->get(0,100)/100.0f;

				continue;
			}

		// CHANGE COLOR SLIGHTLY
			modesum += settings->getCVar("body_percentmutateeffectchangecolor_slightly");
			if ( mode <= modesum )
			{
        //cerr << "Mutate color2." << endl;
				// mutate color
				unsigned int ncolor = randgen->Instance()->get(0,2);
				unsigned int nweight = randgen->Instance()->get(1,10);
				float amount = 0.01 * nweight;

				if ( randgen->Instance()->get(0,1) == 0 )
					amount *= -1;

				if ( ncolor == 0 )
					color.r += amount;
				else if ( ncolor == 1 )
					color.g += amount;
				else if ( ncolor == 2 )
					color.b += amount;
				
				if ( color.r < 0.1f )
				{
					float diff = 0.1f - color.r;
					color.r += diff; color.g += diff; color.b += diff; color.a += diff;
				}
				if ( color.g < 0.1f )
				{
					float diff = 0.1f - color.g;
					color.r += diff; color.g += diff; color.b += diff; color.a += diff;
				}
				if ( color.b < 0.1f )
				{
					float diff = 0.1f - color.b;
					color.r += diff; color.g += diff; color.b += diff; color.a += diff;
				}
			
				if ( color.r > 1.0f && color.g > 1.0f && color.b > 1.0f )
					color.normalize();

				continue;
			}

		// ADD BODYPART
			modesum += settings->getCVar("body_percentmutateeffectaddbodypart");
			if ( mode <= modesum )
			{
        //cerr << "Add bodypart." << endl;
				if ( archBodyparts.size() < settings->getCVar("body_maxbodyparts") )
				{
// 					cerr << "adding bodypart" << endl;
					addRandomBodypart();
// 					cerr << "done adding bodypart" << endl;
				}
				else
					runs++;
				continue;
			}

		// REMOVE BODYPART
			modesum += settings->getCVar("body_percentmutateeffectremovebodypart");
			if ( mode <= modesum )
			{
				if ( archBodyparts.size() > settings->getCVar("body_minbodyparts") )
				{
					// pick a random bodypart
					unsigned int bid = randgen->Instance()->get( 0, archBodyparts.size()-1 );

					// if not main body, remove it
					if ( archBodyparts[bid].id != 1000 )
					{
 						//cerr << "removing bodypart " << bid << " id " << archBodyparts[bid].id  << endl;

						removeBodypart( archBodyparts[bid].id );

// 						cerr << "removing obsolete constraints, expected errors:" << endl;
						for ( int i = 0; i < (int)archConstraints.size(); i++ )
						{
							archConstraint* c = &archConstraints[i];
							if ( findBodypart( c->id_1 ) == -1 )
							{
								archConstraints.erase(archConstraints.begin()+i);
								i--;
							}
							else if ( findBodypart( c->id_2 ) == -1 )
							{
								archConstraints.erase(archConstraints.begin()+i);
								i--;
							}
						}
					}
					else
						runs++;

				}
				else
					runs++;
				continue;
			}

		// RESIZE BODYPART
			modesum += settings->getCVar("body_percentmutateeffectresizebodypart");
			if ( mode <= modesum )
			{
   				//cerr << "resize bodypart" << endl;

					// pick a random bodypart
					unsigned int bid = randgen->Instance()->get( 0, archBodyparts.size()-1 );
					archBodypart *bp = &archBodyparts[bid];

					unsigned int axismode = randgen->Instance()->get(0,2);
					if ( axismode == 0 )
						bp->x = randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );
					else if ( axismode == 1 )
						bp->y = randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );
					else
						bp->z = randgen->Instance()->get( settings->getCVar("body_minbodypartsize"), settings->getCVar("body_maxbodypartsize") );

					// reposition the constraints back to the resized bodypart
					repositiontoConstraints(bp);

// 				cerr << "done resize bodypart" << endl;
				continue;
			}

		// RESIZE BODYPART SLIGHTLY
			modesum += settings->getCVar("body_percentmutateeffectresizebodypart_slightly");
			if ( mode <= modesum )
			{
   				//cerr << "resize bodypart slightly" << endl;

					// pick a random bodypart
					unsigned int bid = randgen->Instance()->get( 0, archBodyparts.size()-1 );
					archBodypart* bp = &archBodyparts[bid];

					unsigned int axismode = randgen->Instance()->get(0,2);
					unsigned int direction = randgen->Instance()->get(0,1);
					if ( axismode == 0 )
					{
						if ( direction == 0 )
							bp->x += randgen->Instance()->get( 1, settings->getCVar("body_maxbodypartsize") / 10 );
						else
							bp->x -= randgen->Instance()->get( 1, settings->getCVar("body_maxbodypartsize") / 10 );
					}
					else if ( axismode == 1 )
					{
						if ( direction == 0 )
							bp->y += randgen->Instance()->get( 1, settings->getCVar("body_maxbodypartsize") / 10 );
						else
							bp->y -= randgen->Instance()->get( 1, settings->getCVar("body_maxbodypartsize") / 10 );
					}
					else
					{
						if ( direction == 0 )
							bp->z += randgen->Instance()->get( 1, settings->getCVar("body_maxbodypartsize") / 10 );
						else
							bp->z -= randgen->Instance()->get( 1, settings->getCVar("body_maxbodypartsize") / 10 );
					}

					// see that they didn't go over their limits
					if ( bp->x < settings->getCVar("body_minbodypartsize") )
						bp->x = settings->getCVar("body_minbodypartsize");
					else if ( bp->x > settings->getCVar("body_maxbodypartsize") )
						bp->x = settings->getCVar("body_minbodypartsize");

					if ( bp->y < settings->getCVar("body_minbodypartsize") )
						bp->y = settings->getCVar("body_minbodypartsize");
					else if ( bp->y > settings->getCVar("body_maxbodypartsize") )
						bp->y = settings->getCVar("body_minbodypartsize");

					if ( bp->z < settings->getCVar("body_minbodypartsize") )
						bp->z = settings->getCVar("body_minbodypartsize");
					else if ( bp->z > settings->getCVar("body_maxbodypartsize") )
						bp->z = settings->getCVar("body_minbodypartsize");

					// reposition the constraints back to the resized bodypart
					repositiontoConstraints(bp);

// 				cerr << "done resize bodypart" << endl;
				continue;
			}

		// CHANGE CONSTRAINT LIMITS
			modesum += settings->getCVar("body_percentmutateeffectchangeconstraintlimits");
			if ( mode <= modesum && archConstraints.size() != 0) // 0 constraints in 1-bodypart critter.
			{

				unsigned int cid = randgen->Instance()->get( 0, archConstraints.size()-1 );
				archConstraint* co = &archConstraints[cid];
				co->limit_1		= (float)randgen->Instance()->get( 0, 7853 ) / -10000;
				co->limit_2		= -1.0f * co->limit_1;

// 				cerr << "done changing constraint limits" << endl;
				continue;
			}
	
		// CHANGE CONSTRAINT LIMITS SLIGHTLY
			modesum += settings->getCVar("body_percentmutateeffectchangeconstraintlimits_slightly");
			if ( mode <= modesum && archConstraints.size() != 0) // 0 constraints in 1-bodypart critter.
			{
				unsigned int cid = randgen->Instance()->get( 0, archConstraints.size()-1 );
				archConstraint* co = &archConstraints[cid];
				
				unsigned int direction = randgen->Instance()->get(0,1);

				if ( direction == 0 )
					co->limit_1 += 0.01f;
				else
					co->limit_1 -= 0.01f;
				// check limits limits, ya
				if ( co->limit_1 > 0.0f )
					co->limit_1 = 0.0f;
				if ( co->limit_1 < -0.7853f )
					co->limit_1 = -0.7853f;

				co->limit_2 = -1.0f * co->limit_1;

// 				cerr << "done changing constraint limits" << endl;
				continue;
			}
		// CHANGE CONSTRAINT ANGLES
			modesum += settings->getCVar("body_percentmutateeffectchangeconstraintangles");
			if ( mode <= modesum && archConstraints.size() != 0) // 0 constraints in 1-bodypart critter.
			{

				unsigned int cid = randgen->Instance()->get( 0, archConstraints.size()-1 );
				archConstraint* co = &archConstraints[cid];
				co->rot_x_1		= ((float)randgen->Instance()->get( 0, 3141 ) - 1571) / 1000;
				co->rot_z_1		= ((float)randgen->Instance()->get( 0, 3141 ) - 1571) / 1000;
				co->rot_y_1		= ((float)randgen->Instance()->get( 0, 3141 ) - 1571) / 1000;

				co->rot_x_2		= ((float)randgen->Instance()->get( 0, 3141 ) - 1571) / 1000;
				co->rot_y_2		= ((float)randgen->Instance()->get( 0, 3141 ) - 1571) / 1000;
				co->rot_z_2		= ((float)randgen->Instance()->get( 0, 3141 ) - 1571) / 1000;

// 				cerr << "done changing constraint angles" << endl;
				continue;
			}

		// CHANGE CONSTRAINT ANGLES SLIGHTLY
			modesum += settings->getCVar("body_percentmutateeffectchangeconstraintangles_slightly");
			if ( mode <= modesum && archConstraints.size() != 0) // 0 constraints in 1-bodypart critter.
			{

				unsigned int cid = randgen->Instance()->get( 0, archConstraints.size()-1 );
				archConstraint* co = &archConstraints[cid];

				unsigned int XYZ = randgen->Instance()->get(0,2);
				unsigned int direction = randgen->Instance()->get(0,1);
				if ( direction == 0 )
				{
					if ( XYZ == 0 )
						co->rot_x_1 += 0.01f;
					else if ( XYZ == 1 )
						co->rot_y_1 += 0.01f;
					else
						co->rot_z_1 += 0.01f;
				}
				else
				{
					if ( XYZ == 0 )
						co->rot_x_1 -= 0.01f;
					else if ( XYZ == 1 )
						co->rot_y_1 -= 0.01f;
					else
						co->rot_z_1 -= 0.01f;
				}

				XYZ = randgen->Instance()->get(0,2);
				direction = randgen->Instance()->get(0,1);
				if ( direction == 0 )
				{
					if ( XYZ == 0 )
						co->rot_x_2 += 0.01f;
					else if ( XYZ == 1 )
						co->rot_y_2 += 0.01f;
					else
						co->rot_z_2 += 0.01f;
				}
				else
				{
					if ( XYZ == 0 )
						co->rot_x_2 -= 0.01f;
					else if ( XYZ == 1 )
						co->rot_y_2 -= 0.01f;
					else
						co->rot_z_2 -= 0.01f;
				}

				if ( co->rot_x_1 < -0.1571f )
					co->rot_x_1 = -0.1571f;
				if ( co->rot_x_1 > 0.1571f )
					co->rot_x_1 = 0.1571f;
				if ( co->rot_y_1 < -0.1571f )
					co->rot_y_1 = -0.1571f;
				if ( co->rot_y_1 > 0.1571f )
					co->rot_y_1 = 0.1571f;
				if ( co->rot_z_1 < -0.1571f )
					co->rot_z_1 = -0.1571f;
				if ( co->rot_z_1 > 0.1571f )
					co->rot_z_1 = 0.1571f;

				if ( co->rot_x_2 < -0.1571f )
					co->rot_x_2 = -0.1571f;
				if ( co->rot_x_2 > 0.1571f )
					co->rot_x_2 = 0.1571f;
				if ( co->rot_y_2 < -0.1571f )
					co->rot_y_2 = -0.1571f;
				if ( co->rot_y_2 > 0.1571f )
					co->rot_y_2 = 0.1571f;
				if ( co->rot_z_2 < -0.1571f )
					co->rot_z_2 = -0.1571f;
				if ( co->rot_z_2 > 0.1571f )
					co->rot_z_2 = 0.1571f;

// 				cerr << "done changing constraint angles" << endl;
				continue;
			}

		// REPOSITION A CONSTRAINT
			modesum += settings->getCVar("body_percentmutateeffectchangeconstraintposition");
			if ( mode <= modesum && archConstraints.size() != 0) // 0 constraints in 1-bodypart critter.
			{


				unsigned int cid = randgen->Instance()->get( 0, archConstraints.size()-1 );
				archConstraint* co = &archConstraints[cid];

				int connID1 = findBodypart(co->id_1);
				int connID2 = findBodypart(co->id_2);

				// pick one of 2 bodies to reconnect
				unsigned int body1or2 = randgen->Instance()->get( 1, 2 );

				if ( body1or2 == 1 )
					randomConstraintPosition(co, 1, connID1);
				else
					randomConstraintPosition(co, 2, connID2);

// 				cerr << "done changing constraint position" << endl;
				continue;
			}

		// REPOSITION A CONSTRAINT SLIGHTLY
			modesum += settings->getCVar("body_percentmutateeffectchangeconstraintposition_slightly");
			if ( mode <= modesum && archConstraints.size() != 0) // 0 constraints in 1-bodypart critter.
			{

				unsigned int cid = randgen->Instance()->get( 0, archConstraints.size()-1 );
				archConstraint* co = &archConstraints[cid];

				int connID1 = findBodypart(co->id_1);
				int connID2 = findBodypart(co->id_2);

				// pick one of 2 bodies to reconnect
				unsigned int body1or2 = randgen->Instance()->get( 1, 2 );
				unsigned int direction = randgen->Instance()->get( 0, 1 );
				unsigned int axis1or2 = randgen->Instance()->get( 0, 1 );

				if ( body1or2 == 1 ) {
					// now we know the plane to connect to, determine positions
					if ( co->XYZ == 0 ) { // X
						if ( direction == 0 ) {
							if ( axis1or2 == 0 )	co->pos_y_1 += archBodyparts[connID1].y / 100000;
							else			co->pos_z_1 += archBodyparts[connID1].z / 100000;
						} else {
							if ( axis1or2 == 0 )	co->pos_y_1 -= archBodyparts[connID1].y / 100000;
							else			co->pos_z_1 -= archBodyparts[connID1].z / 100000;
						}
					}
					else if ( co->XYZ == 1 ) { // Y
						if ( direction == 0 ) {
							if ( axis1or2 == 0 )	co->pos_x_1 += archBodyparts[connID1].x / 100000;
							else			co->pos_z_1 += archBodyparts[connID1].z / 100000;
						} else {
							if ( axis1or2 == 0 )	co->pos_x_1 -= archBodyparts[connID1].x / 100000;
							else			co->pos_z_1 -= archBodyparts[connID1].z / 100000;
						}
					}
					else { // Z
						if ( direction == 0 ) {
							if ( axis1or2 == 0 )	co->pos_x_1 += archBodyparts[connID1].x / 100000;
							else			co->pos_y_1 += archBodyparts[connID1].y / 100000;
						} else {
							if ( axis1or2 == 0 )	co->pos_x_1 -= archBodyparts[connID1].x / 100000;
							else			co->pos_y_1 -= archBodyparts[connID1].y / 100000;
						}
					}
					if ( co->pos_x_1 < archBodyparts[connID1].x/-2000 )
						co->pos_x_1 = archBodyparts[connID1].x/-2000;
					if ( co->pos_x_1 > archBodyparts[connID1].x/2000 )
						co->pos_x_1 = archBodyparts[connID1].x/2000;
					if ( co->pos_y_1 < archBodyparts[connID1].y/-2000 )
						co->pos_y_1 = archBodyparts[connID1].y/-2000;
					if ( co->pos_y_1 > archBodyparts[connID1].y/2000 )
						co->pos_y_1 = archBodyparts[connID1].y/2000;
					if ( co->pos_z_1 < archBodyparts[connID1].z/-2000 )
						co->pos_z_1 = archBodyparts[connID1].z/-2000;
					if ( co->pos_z_1 > archBodyparts[connID1].z/2000 )
						co->pos_z_1 = archBodyparts[connID1].z/2000;
				}
				else {
          // now we know the plane to connect to, determine positions
          if ( co->XYZ == 0 ) { // X
            if ( direction == 0 ) {
              if ( axis1or2 == 0 )	co->pos_y_2 += archBodyparts[connID2].y / 100000;
              else			co->pos_z_2 += archBodyparts[connID2].z / 100000;
            } else {
              if ( axis1or2 == 0 )	co->pos_y_2 -= archBodyparts[connID2].y / 100000;
              else			co->pos_z_2 -= archBodyparts[connID2].z / 100000;
            }
          }
          else if ( co->XYZ == 1 ) { // Y
            if ( direction == 0 ) {
              if ( axis1or2 == 0 )	co->pos_x_2 += archBodyparts[connID2].x / 100000;
              else			co->pos_z_2 += archBodyparts[connID2].z / 100000;
            } else {
              if ( axis1or2 == 0 )	co->pos_x_2 -= archBodyparts[connID2].x / 100000;
              else			co->pos_z_2 -= archBodyparts[connID2].z / 100000;
            }
          }
          else { // Z
            if ( direction == 0 ) {
              if ( axis1or2 == 0 )	co->pos_x_2 += archBodyparts[connID2].x / 100000;
              else			co->pos_y_2 += archBodyparts[connID2].y / 100000;
            } else {
              if ( axis1or2 == 0 )	co->pos_x_2 -= archBodyparts[connID2].x / 100000;
              else			co->pos_y_2 -= archBodyparts[connID2].y / 100000;
            }
          }
          if ( co->pos_x_2 < archBodyparts[connID2].x/-2000 )
            co->pos_x_2 = archBodyparts[connID2].x/-2000;
          if ( co->pos_x_2 > archBodyparts[connID2].x/2000 )
            co->pos_x_2 = archBodyparts[connID2].x/2000;
          if ( co->pos_y_2 < archBodyparts[connID2].y/-2000 )
            co->pos_y_2 = archBodyparts[connID2].y/-2000;
          if ( co->pos_y_2 > archBodyparts[connID2].y/2000 )
            co->pos_y_2 = archBodyparts[connID2].y/2000;
          if ( co->pos_z_2 < archBodyparts[connID2].z/-2000 )
            co->pos_z_2 = archBodyparts[connID2].z/-2000;
          if ( co->pos_z_2 > archBodyparts[connID2].z/2000 )
            co->pos_z_2 = archBodyparts[connID2].z/2000;
        
				}
					
// 					randomConstraintPosition(co, 2, connID2);

// 				cerr << "done changing constraint position" << endl;
				continue;
			}

		// if we reach here, none were processed, decrease runs by 1 to make sure we get a hit
			if ( modesum > 0 )
				runs++;
	}
}

void BodyArch::repositiontoConstraints( archBodypart* bp )
{
	// reposition the constraints back to the resized bodypart / mouth
  if(archConstraints.size() == 0)
    if(archBodyparts.size() != 1)
      cerr << "No constraints in reposition " << archBodyparts.size() << " parts." << endl;

  // cerr << "repositioning bodypart " << bp->id << endl;

	for ( unsigned int i = 0; i < archConstraints.size(); i++ )
	{
		archConstraint* co = &archConstraints[i];
		if ( findBodypart( co->id_1 ) == bp->id )
		{
			if ( co->XYZ == 0 ) // X
				co->pos_x_1 = (bp->x / 2000.0f) * co->sign * bodypartspacer;
			else if ( co->XYZ == 1 ) // Y
				co->pos_y_1 = (bp->y / 2000.0f) * co->sign * bodypartspacer;
			else if ( co->XYZ == 2 ) // Z
				co->pos_z_1 = (bp->z / 2000.0f) * co->sign * bodypartspacer;
		}
		else if ( findBodypart( co->id_2 ) == bp->id )
		{
			int othersign = -1 * co->sign;
			if ( co->XYZ == 0 ) // X
				co->pos_x_2 = (bp->x / 2000.0f) * othersign * bodypartspacer;
			else if ( co->XYZ == 1 ) // Y
				co->pos_y_2 = (bp->y / 2000.0f) * othersign * bodypartspacer;
			else if ( co->XYZ == 2 ) // Z
				co->pos_z_2 = (bp->z / 2000.0f) * othersign * bodypartspacer;
		}
	}
}

int BodyArch::findBodypart( unsigned int id )
{
	for ( unsigned int i=0; i < archBodyparts.size(); i++ )
	{
		if ( archBodyparts[i].id == id )
		{
			return i;
		}
	}
// 	cerr << " NOT GOOD: cannot find a bodypart for the id " << id << endl;
	return -1;
}

unsigned int BodyArch::getUniqueBodypartID()
{
	unsigned int id = 1000;
	bool found = true;
	while ( found )
	{
		found = false;
		for ( unsigned int i = 0; i < archBodyparts.size() && !found; i++ )
			if ( archBodyparts[i].id == id )
			{
				found = true;
				id++;
			}
	}
	return id;
}

unsigned int BodyArch::getUniqueConstraintID()
{
	unsigned int id = 1000;
	bool found = true;
	while ( found )
	{
		found = false;
		for ( unsigned int i = 0; i < archConstraints.size() && !found; i++ )
		{
			if ( archConstraints[i].constraint_id1 == id || archConstraints[i].constraint_id2 == id )
			{
				found = true;
				id++;
			}
		}
	}
	return id;
}

void BodyArch::copyFrom(const BodyArch* otherBody)
{
	color = otherBody->color;
	retinasize = otherBody->retinasize;

	for ( unsigned int i=0; i < otherBody->archBodyparts.size(); i++ )
	{
		const archBodypart *obp = &otherBody->archBodyparts[i];
		archBodyparts.push_back( archBodypart() );
		archBodypart *bp = &archBodyparts[archBodyparts.size()-1];
		
		bp->id		= obp->id;
		bp->type	= obp->type;
		bp->materialID	= obp->materialID;
		bp->x		= obp->x;
		bp->y		= obp->y;
		bp->z		= obp->z;
	}

	for ( unsigned int i=0; i < otherBody->archConstraints.size(); i++ )
	{
		const archConstraint *oco = &otherBody->archConstraints[i];
		archConstraints.push_back( archConstraint() );
		archConstraint *co = &archConstraints[archConstraints.size()-1];
		
		co->constraint_id1	= oco->constraint_id1;
		co->constraint_id2	= oco->constraint_id2;
		co->XYZ			= oco->XYZ;
		co->sign		= oco->sign;
		co->id_1		= oco->id_1;
		co->id_2		= oco->id_2;
		co->rot_x_1		= oco->rot_x_1;
		co->rot_x_2		= oco->rot_x_2;
		co->rot_y_1		= oco->rot_y_1;
		co->rot_y_2		= oco->rot_y_2;
		co->rot_z_1		= oco->rot_z_1;
		co->rot_z_2		= oco->rot_z_2;
		co->pos_x_1		= oco->pos_x_1;
		co->pos_x_2		= oco->pos_x_2;
		co->pos_y_1		= oco->pos_y_1;
		co->pos_y_2		= oco->pos_y_2;
		co->pos_z_1		= oco->pos_z_1;
		co->pos_z_2		= oco->pos_z_2;
		co->limit_1		= oco->limit_1;
		co->limit_2		= oco->limit_2;
	}
}

void BodyArch::setArch(string* content)
{
//	cerr << *content << endl;

	string contentCopy = *content;
	string line = parseH.returnUntillStrip( "\n", contentCopy );
	while ( !line.empty() )
	{
		if ( parseH.beginMatchesStrip( "color=", line ) )
		{
			string R = parseH.returnUntillStrip( ",", line );
			string G = parseH.returnUntillStrip( ",", line );
			string B = parseH.returnUntillStrip( ";", line );

			if(EOF == sscanf(R.c_str(), "%f", &color.r)) cerr << "ERROR INSERTING CRITTER (colorR)" << endl;
			if(EOF == sscanf(G.c_str(), "%f", &color.g)) cerr << "ERROR INSERTING CRITTER (colorG)" << endl;
			if(EOF == sscanf(B.c_str(), "%f", &color.b)) cerr << "ERROR INSERTING CRITTER (colorB)" << endl;
			color.a = 0.0f;
		}
		else if ( parseH.beginMatchesStrip( "retinasize=", line ) )
		{
			string RES = parseH.returnUntillStrip( ";", line );
			//cerr << "RES: " << RES << endl;
			if(EOF == sscanf(RES.c_str(), "%d", &retinasize)) cerr << "ERROR INSERTING CRITTER" << endl;
		}
		else if ( parseH.beginMatchesStrip( "b ", line ) )
		{
      // BODYPART

			archBodyparts.push_back( archBodypart() );
			archBodypart *bp = &archBodyparts[archBodyparts.size()-1];
			
      // id
			string bodypartID = parseH.returnUntillStrip( " ", line );
			if(EOF == sscanf(bodypartID.c_str(), "%d", &bp->id)) cerr << "error in parsing body" << endl;
      // type
			string bodypartType = parseH.returnUntillStrip( " ", line );
      int temp = 0;
      if(EOF == sscanf(bodypartType.c_str(), "%d", &temp)) cerr << "error in parsing type (s/box/0/)" << endl;
      switch(temp)
      {
        case 1: bp->type = archBodypart::HEAD; break;
        default:
        case 0: bp->type = archBodypart::DEFAULT; break;
      }

			// materialID
			string materialID = parseH.returnUntillStrip( " ", line );
			if(EOF == sscanf(materialID.c_str(), "%d", &bp->materialID)) cerr << "error in parsing body" << endl;

			// dimesions X Y Z
			string X = parseH.returnUntillStrip( " ", line );
			string Y = parseH.returnUntillStrip( " ", line );
			string Z = line;
			if(EOF == sscanf(X.c_str(), "%f", &bp->x)) cerr << "error in parsing body" << endl;
			if(EOF == sscanf(Y.c_str(), "%f", &bp->y)) cerr << "error in parsing body" << endl;
			if(EOF == sscanf(Z.c_str(), "%f", &bp->z)) cerr << "error in parsing body" << endl;

		}
		else if ( parseH.beginMatchesStrip( "c ", line ) )
		{
      // CONSTRAINT

			archConstraint CO;
			archConstraints.push_back( CO );
			archConstraint *co = &archConstraints[archConstraints.size()-1];
			
			// XYZ & sign
				string XYZ = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(XYZ.c_str(), "%d", &co->XYZ)) cerr << "error in parsing body" << endl;
				string SIGN = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(SIGN.c_str(), "%d", &co->sign)) cerr << "error in parsing body" << endl;

			// CONSTRAINT IDS
				string ID = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ID.c_str(), "%d", &co->constraint_id1)) cerr << "error in parsing body" << endl;
				ID = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ID.c_str(), "%d", &co->constraint_id2)) cerr << "error in parsing body" << endl;

			// CONNECTION TO BODYPART 1
				string ID_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ID_1.c_str(), "%d", &co->id_1)) cerr << "error in parsing body" << endl;

			// ROTATION
				string ROT_X_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ROT_X_1.c_str(), "%f", &co->rot_x_1)) cerr << "error in parsing body" << endl;
				string ROT_Y_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ROT_Y_1.c_str(), "%f", &co->rot_y_1)) cerr << "error in parsing body" << endl;
				string ROT_Z_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ROT_Z_1.c_str(), "%f", &co->rot_z_1)) cerr << "error in parsing body" << endl;

			// POSITION
				string POS_X_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(POS_X_1.c_str(), "%f", &co->pos_x_1)) cerr << "error in parsing body" << endl;
				string POS_Y_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(POS_Y_1.c_str(), "%f", &co->pos_y_1)) cerr << "error in parsing body" << endl;
				string POS_Z_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(POS_Z_1.c_str(), "%f", &co->pos_z_1)) cerr << "error in parsing body" << endl;

			// LIMIT
				string LIMIT_1 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(LIMIT_1.c_str(), "%f", &co->limit_1)) cerr << "error in parsing body" << endl;

			// CONNECTION TO BODYPART 2
				string ID_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ID_2.c_str(), "%d", &co->id_2)) cerr << "error in parsing body" << endl;

			// ROTATION
				string ROT_X_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ROT_X_2.c_str(), "%f", &co->rot_x_2)) cerr << "error in parsing body" << endl;
				string ROT_Y_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ROT_Y_2.c_str(), "%f", &co->rot_y_2)) cerr << "error in parsing body" << endl;
				string ROT_Z_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(ROT_Z_2.c_str(), "%f", &co->rot_z_2)) cerr << "error in parsing body" << endl;

			// POSITION
				string POS_X_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(POS_X_2.c_str(), "%f", &co->pos_x_2)) cerr << "error in parsing body" << endl;
				string POS_Y_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(POS_Y_2.c_str(), "%f", &co->pos_y_2)) cerr << "error in parsing body" << endl;
				string POS_Z_2 = parseH.returnUntillStrip( " ", line );
				if(EOF == sscanf(POS_Z_2.c_str(), "%f", &co->pos_z_2)) cerr << "error in parsing body" << endl;

			// LIMIT
				string LIMIT_2 = line;
				if(EOF == sscanf(LIMIT_2.c_str(), "%f", &co->limit_2)) cerr << "error in parsing body" << endl;
		}
		else if ( parseH.beginMatchesStrip( "cm ", line ) )
		{
      cerr << "Error, outdated critter data." << endl;
		}
		else if ( parseH.beginMatchesStrip( "m ", line ) )
		{
      cerr << "Error, outdated critter data." << endl;
		}
		line = parseH.returnUntillStrip( "\n", contentCopy );
	}
}


string* BodyArch::getArch()
{
	stringstream buf;

	// neuron info
		buf << "color=" << color.r << "," << color.g << "," << color.b << ";\n";
		buf << "retinasize=" << retinasize << ";\n";
		for ( unsigned int i=0; i < archBodyparts.size(); i++ )
		{
			archBodypart *bp = &archBodyparts[i];
      int temp = 0;
      switch(bp->type)
      {
        case archBodypart::HEAD: temp = 1; break;
        default:
          temp = 0;
      }
			buf << "b" << " " << bp->id << " " << temp << " " << bp->materialID << " " << bp->x << " " << bp->y << " " << bp->z << "\n";
		}

		for ( unsigned int i=0; i < archConstraints.size(); i++ )
		{
			archConstraint *co = &archConstraints[i];
  		buf << "c";
			buf << " " << co->XYZ << " " << co->sign;
			buf << " " << co->constraint_id1 << " " << co->constraint_id2 << " ";
			buf << co->id_1 << " " << co->rot_x_1 << " " << co->rot_y_1 << " " << co->rot_z_1 << " " << co->pos_x_1 << " " << co->pos_y_1 << " " << co->pos_z_1 << " " << co->limit_1 << " ";
			buf << co->id_2 << " " << co->rot_x_2 << " " << co->rot_y_2 << " " << co->rot_z_2 << " " << co->pos_x_2 << " " << co->pos_y_2 << " " << co->pos_z_2 << " " << co->limit_2;
			buf << "\n";
		}

	archBuffer = buf.str();
	return &archBuffer;
}

BodyArch::~BodyArch()
{
}
